#include <iostream>
#include <cv.h>
#include <highgui.h>

//finds the red notes in guitar hero
void findRed(IplImage* img);

//finds the red notes in guitar hero
void findBlue(IplImage* img);

//finds the green notes in guitar hero
void findGreen(IplImage* img);

//finds the yellow notes in guitar hero
void findYellow(IplImage* img);

//find the orange notes in guitar hero
void findOrange(IplImage* img);

//a test function to figure out working color combinations
void findTest(IplImage* img);

IplImage* cropImage(IplImage* img, int top, int bottom, int left, int right);

void captureTest();

using namespace std;

int main(char ** argv)
{
	cout << "wtf";
	captureTest();
	cvNamedWindow("Image");
	//IplImage* src = cvLoadImage("test2.jpg");
	//findRed(src);
	//findBlue(src);
	//findGreen(src);
	//findYellow(src);
	//findOrange(src);
	//findTest(src);
	//cvShowImage("Image", src);
	cvWaitKey(0);
}

void findRed(IplImage* img) 
{
	IplImage* src;
	IplImage* src_red;
	IplImage* src_red_binary;
	src = img;


	src_red = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);

	//converts all values into the red value of the original image image is arranged by BGR for each channel
	for (int i = 0; i < src_red->height; i++)
	{
		for (int j = 0; j < src_red->width; j++) 
		{
			unsigned char R = src->imageData[i * src->widthStep + j * src->nChannels + 2];
			unsigned char B = src->imageData[i * src->widthStep + j * src->nChannels];
			unsigned char G = src->imageData[i * src->widthStep + j * src->nChannels + 1];
			src_red->imageData[i * src_red->width + j] = R;
			//if(R > 200 && G > 200 && B > 200)
			//	src_red->imageData[i * src_red->width + j] = 200;
			if(G > 100 || B > 150)
				src_red->imageData[i * src_red->width + j] = 0;
		}
	}


	src_red_binary = cvCreateImage(cvSize(src->width, src->height), src->depth,
			1);


	cvThreshold(src_red, src_red_binary, 160, 255, CV_THRESH_BINARY);

	// Find Contours
	IplImage* src_contours = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);
	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* contour = 0;
	CvRect rect;
	
	cvZero(src_contours);
	cvFindContours(src_red_binary, storage, &contour, sizeof(CvContour), 
			CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != 0; contour = contour->h_next) 
	{
		rect = cvBoundingRect(contour, 1);
		if (	(rect.width + rect.height > 5) && !(
				//((rect.height / rect.width) < 1.1) &&
				((rect.width / rect.height) < 1.1)))
		{
			cvDrawContours(src_contours, contour, CV_RGB(2, 198, 255), CV_RGB(2, 198, 255), -1, CV_FILLED, 8);
			// Bound cone by green square
			cvRectangle(src, cvPoint(rect.x, rect.y), cvPoint(rect.x
					+ rect.width, rect.y + rect.height), CV_RGB(255, 0, 0), 5);
		}
	}
	
	cvReleaseMemStorage(&storage);
	cvReleaseImage(&src_contours);
	cvReleaseImage(&src_red);
	cvReleaseImage(&src_red_binary);
}

void findBlue(IplImage* img) 
{
	IplImage* src;
	IplImage* src_red;
	IplImage* src_red_binary;
	src = img;

	src_red = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);

	//converts all values into the red value of the original image image is arranged by BGR for each channel
	for (int i = 0; i < src_red->height; i++)
	{
		for (int j = 0; j < src_red->width; j++) 
		{
			unsigned char R = src->imageData[i * src->widthStep + j * src->nChannels + 2];
			unsigned char B = src->imageData[i * src->widthStep + j * src->nChannels];
			unsigned char G = src->imageData[i * src->widthStep + j * src->nChannels + 1];
			src_red->imageData[i * src_red->width + j]
			                   = B;
			if(R > 75 || G > 200)
				src_red->imageData[i * src_red->width + j] = 0;
		}
	}


	src_red_binary = cvCreateImage(cvSize(src->width, src->height), src->depth,
			1);


	cvThreshold(src_red, src_red_binary, 150, 255, CV_THRESH_BINARY);

	// Find Contours
	IplImage* src_contours = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);
	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* contour = 0;
	CvRect rect;
	
	cvZero(src_contours);
	cvFindContours(src_red_binary, storage, &contour, sizeof(CvContour), 
			CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != 0; contour = contour->h_next) 
	{
		rect = cvBoundingRect(contour, 1);
		if (	(rect.width + rect.height > 5) &&	!(
				//((rect.height / rect.width) < 1.1) &&
				((rect.width / rect.height) < 1.1)))
		{
			cvDrawContours(src_contours, contour, CV_RGB(2, 198, 255), CV_RGB(2, 198, 255), -1, CV_FILLED, 8);
			// Bound cone by green square
			cvRectangle(src, cvPoint(rect.x, rect.y), cvPoint(rect.x
					+ rect.width, rect.y + rect.height), CV_RGB(0, 0, 255), 5);
		}
	}

	cvReleaseMemStorage(&storage);
	cvReleaseImage(&src_contours);
	cvReleaseImage(&src_red);
	cvReleaseImage(&src_red_binary);
}

void findGreen(IplImage* src) {
	
	IplImage* src_red;
	IplImage* src_red_binary;
	src_red = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);

	//converts all values into the red value of the original image image is arranged by BGR for each channel
	for (int i = 0; i < src_red->height; i++)
	{
		for (int j = 0; j < src_red->width; j++) 
		{
			unsigned char R = src->imageData[i * src->widthStep + j * src->nChannels + 2];
			unsigned char B = src->imageData[i * src->widthStep + j * src->nChannels];
			unsigned char G = src->imageData[i * src->widthStep + j * src->nChannels + 1];
			src_red->imageData[i * src_red->width + j]
			                   = G;
			if(R > 150 || B > 150)
				src_red->imageData[i * src_red->width + j] = 0;
		}
	}


	src_red_binary = cvCreateImage(cvSize(src->width, src->height), src->depth,
			1);


	cvThreshold(src_red, src_red_binary, 160, 255, CV_THRESH_BINARY);

	// Find Contours
	IplImage* src_contours = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);
	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* contour = 0;
	CvRect rect;
	
	cvZero(src_contours);
	cvFindContours(src_red_binary, storage, &contour, sizeof(CvContour), 
			CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != 0; contour = contour->h_next) {
		rect = cvBoundingRect(contour, 1);
		if (	(rect.width + rect.height > 5) &&	!(
				//((rect.height / rect.width) < 1.1) &&
				((rect.width / rect.height) < 1.1)))
		{
			cvDrawContours(src_contours, contour, CV_RGB(2, 198, 255), CV_RGB(2, 198, 255), -1, CV_FILLED, 8);
			// Bound cone by green square
			cvRectangle(src, cvPoint(rect.x, rect.y), cvPoint(rect.x
					+ rect.width, rect.y + rect.height), CV_RGB(0, 255, 0), 5);
		}
	}	
	
	cvReleaseMemStorage(&storage);
	cvReleaseImage(&src_contours);
	cvReleaseImage(&src_red);
	cvReleaseImage(&src_red_binary);
}

void findYellow(IplImage* img) 
{
	IplImage* src;
	IplImage* src_red;
	IplImage* src_red_binary;
	src = img;
	src_red = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);

	//converts all values into the red value of the original image image is arranged by BGR for each channel
	for (int i = 0; i < src_red->height; i++)
	{
		for (int j = 0; j < src_red->width; j++) 
		{
			unsigned char R = src->imageData[i * src->widthStep + j * src->nChannels + 2];
			unsigned char B = src->imageData[i * src->widthStep + j * src->nChannels];
			unsigned char G = src->imageData[i * src->widthStep + j * src->nChannels + 1];
			src_red->imageData[i * src_red->width + j]
			                   = G;
			if(R < 150 || B > 100)
				src_red->imageData[i * src_red->width + j] = 0;
		}
	}

	src_red_binary = cvCreateImage(cvSize(src->width, src->height), src->depth,
			1);


	cvThreshold(src_red, src_red_binary, 190, 255, CV_THRESH_BINARY);
	// Find Contours
	IplImage* src_contours = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);
	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* contour = 0;
	CvRect rect;
	cvZero(src_contours);
	cvFindContours(src_red_binary, storage, &contour, sizeof(CvContour), 
			CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != 0; contour = contour->h_next) 
	{
		rect = cvBoundingRect(contour, 1);
		if (	(rect.width + rect.height > 5) &&	!(
				//((rect.height / rect.width) < 1.1) &&
				((rect.width / rect.height) < 1.1)))
		{
			cvDrawContours(src_contours, contour, CV_RGB(2, 198, 255), CV_RGB(2, 198, 255), -1, CV_FILLED, 8);
			// Bound cone by green square
			cvRectangle(src, cvPoint(rect.x, rect.y), cvPoint(rect.x
					+ rect.width, rect.y + rect.height), CV_RGB(255, 255, 0), 5);
		}
	}
	
	cvReleaseMemStorage(&storage);
	cvReleaseImage(&src_contours);
	cvReleaseImage(&src_red);
	cvReleaseImage(&src_red_binary);
}

void findOrange(IplImage* img)
{
	IplImage* src;
	IplImage* src_red;
	IplImage* src_red_binary;
	src = img;
	src_red = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);

	//converts all values into the red value of the original image image is arranged by BGR for each channel
	for (int i = 0; i < src_red->height; i++)
	{
		for (int j = 0; j < src_red->width; j++) 
		{
			unsigned char R = src->imageData[i * src->widthStep + j * src->nChannels + 2];
			unsigned char B = src->imageData[i * src->widthStep + j * src->nChannels];
			unsigned char G = src->imageData[i * src->widthStep + j * src->nChannels + 1];
			src_red->imageData[i * src_red->width + j]
			                   = R;
			if(B > 100 || G > 150 || G < 100)
				src_red->imageData[i * src_red->width + j] = 0;
		}
	}

	src_red_binary = cvCreateImage(cvSize(src->width, src->height), src->depth,
			1);


	cvThreshold(src_red, src_red_binary, 160, 255, CV_THRESH_BINARY);
	// Find Contours
	IplImage* src_contours = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);
	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* contour = 0;
	CvRect rect;
	
	cvZero(src_contours);
	cvFindContours(src_red_binary, storage, &contour, sizeof(CvContour), 
			CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != 0; contour = contour->h_next) 
	{
		rect = cvBoundingRect(contour, 1);
		if ((rect.width + rect.height > 5 && rect.width + rect.height < 100) &&	!(
				//((rect.height / rect.width) < 1.1) &&
				((rect.width / rect.height) < 1.1)))
		{
			cvDrawContours(src_contours, contour, CV_RGB(2, 198, 255), CV_RGB(2, 198, 255), -1, CV_FILLED, 8);
			// Bound cone by green square
			cvRectangle(src, cvPoint(rect.x, rect.y), cvPoint(rect.x
					+ rect.width, rect.y + rect.height), CV_RGB(255, 125, 0), 5);
		}
	}

	cvReleaseMemStorage(&storage);
	cvReleaseImage(&src_contours);
	cvReleaseImage(&src_red);
	cvReleaseImage(&src_red_binary);
		
}
void findTest(IplImage* img) 
{
	cvNamedWindow("Image");
	
	IplImage* src;
	IplImage* src_red;
	IplImage* src_red_binary;
	src = img;

	src = cropImage(src, 0, img->height - 100, 00, img->width - 500);
	
	src_red = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);

	//converts all values into the red value of the original image image is arranged by BGR for each channel
	for (int i = 0; i < src_red->height; i++)
	{
		for (int j = 0; j < src_red->width; j++) 
		{
			unsigned char R = src->imageData[i * src->widthStep + j * src->nChannels + 2];
			unsigned char B = src->imageData[i * src->widthStep + j * src->nChannels];
			unsigned char G = src->imageData[i * src->widthStep + j * src->nChannels + 1];
			src_red->imageData[i * src_red->width + j]
			                   = R;
			if(B > 100 || G > 150 || G < 100)
				src_red->imageData[i * src_red->width + j] = 0;
		}
	}


	src_red_binary = cvCreateImage(cvSize(src->width, src->height), src->depth,
			1);


	cvThreshold(src_red, src_red_binary, 160, 255, CV_THRESH_BINARY);

	// Find Contours
	IplImage* src_contours = cvCreateImage(cvSize(src->width, src->height), src->depth, 1);
	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* contour = 0;
	CvRect rect;
	src_contours = cvCreateImage(cvSize(src_red->width, src_red->height), 
			IPL_DEPTH_8U, 3);
	cvZero(src_contours);
	cvFindContours(src_red_binary, storage, &contour, sizeof(CvContour), 
			CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

	for (; contour != 0; contour = contour->h_next) {
		rect = cvBoundingRect(contour, 1);
		if (	(rect.width + rect.height > 30 && rect.width + rect.height < 100) &&	!(
				//((rect.height / rect.width) < 1.1) &&
				((rect.width / rect.height) < 1.1)))
		{
			cvDrawContours(src_contours, contour, CV_RGB(2, 198, 255), CV_RGB(2, 198, 255), -1, CV_FILLED, 8);
			// Bound cone by green square
			cvRectangle(src, cvPoint(rect.x, rect.y), cvPoint(rect.x
					+ rect.width, rect.y + rect.height), CV_RGB(255, 125, 0), 5);
		}
	}	
	cvShowImage("Image", src);
	cvWaitKey(0);
}

void captureTest()
{
	cvNamedWindow("mainWin");

	CvCapture* capture = cvCaptureFromAVI("vid.avi");

	if(!capture)
		cout << "waah";
	IplImage* img = 0; 
	int nFrames = 2000;
	for(int i=0; i<nFrames ; i++)
	{
		cvGrabFrame(capture);          // capture a frame
		img=cvRetrieveFrame(capture);  // retrieve the captured frame
		//cvWriteFrame(writer,img);      // add the frame to the file
		findGreen(img);
		findRed(img);
		findBlue(img);
		findYellow(img);
		findOrange(img);
		cvShowImage("mainWin", img); 
		cvWaitKey(40);
		//cvReleaseImage(&img);
	}

	cvReleaseCapture(&capture);


}

IplImage* cropImage(IplImage* img, int top, int bottom, int left, int right)
{
	int channels = img->nChannels;
	
	
	int height = bottom - top;
	
	int width = right - left;
	
	cout << height << " " << width << endl;
	
	IplImage * ret = cvCreateImage(cvSize(width, height), img->depth, channels);
	
	int testnum = 0;
	
	for(int i = 0; i < height; i++)
	{
		for(int j = 0; j < width; j++)
		{
			for(int k = 0; k < channels; k++)
			{
				testnum++;
				ret->imageData[i * width * channels + j * channels + k] =
				img->imageData[channels * (i + top) * img->width + (j + left)*channels + k];
			}
		}
	}
	
	cout << testnum << endl;
	
	return ret;
}
